
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sitl.h
  * @author     baiyang
  * @date       2022-11-28
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "sim_ship.h"

#include <stdint.h>
#include <stdbool.h>

#include <sys/time.h>

#include <gcs_mavlink/gcs.h>
#include <common/gp_defines.h>
#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define SITL_NUM_RANGEFINDERS 10

// number of rc output channels
#define SITL_NUM_CHANNELS 32

#if !defined(AIRSPEED_MAX_SENSORS)
#define AIRSPEED_MAX_SENSORS 3
#endif

#ifndef HAL_COMPASS_MAX_SENSORS
#define HAL_COMPASS_MAX_SENSORS 3

#if HAL_COMPASS_MAX_SENSORS > 1
#define COMPASS_MAX_UNREG_DEV 5
#else
#define COMPASS_MAX_UNREG_DEV 0
#endif

#define COMPASS_MAX_INSTANCES HAL_COMPASS_MAX_SENSORS
#define COMPASS_MAX_BACKEND   HAL_COMPASS_MAX_SENSORS

#define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES)
#endif

#ifndef BARO_MAX_INSTANCES
#define BARO_MAX_INSTANCES 3
#endif

#ifndef INS_MAX_INSTANCES
#define INS_MAX_INSTANCES 3
#endif
/*----------------------------------typedef-----------------------------------*/
enum SITL_RCFail {
    SITL_RCFail_None = 0,
    SITL_RCFail_NoPulses = 1,
    SITL_RCFail_Throttle950 = 2,
};

enum GPSHeading {
    GPS_HEADING_NONE = 0,
    GPS_HEADING_HDT  = 1,
    GPS_HEADING_THS  = 2,
    GPS_HEADING_KSXT = 3,
};

struct vector3f_array {
    uint16_t length;
    Vector3f_t *data;
};

struct float_array {
    uint16_t length;
    float *data;
};

struct sitl_fdm {
    // this is the structure passed between FDM models and the main SITL code
    uint64_t timestamp_us;
    Location home;
    double latitude, longitude; // degrees
    double altitude;  // MSL
    double heading;   // degrees
    double speedN, speedE, speedD; // m/s
    double xAccel, yAccel, zAccel;       // m/s/s in body frame
    double rollRate, pitchRate, yawRate; // degrees/s in body frame
    double rollDeg, pitchDeg, yawDeg;    // euler angles, degrees
    Quat_t quaternion;
    double airspeed; // m/s
    Vector3f_t velocity_air_bf; // velocity relative to airmass, body frame
    double battery_voltage; // Volts
    double battery_current; // Amps
    double battery_remaining; // Ah, if non-zero capacity
    uint8_t num_motors;
    uint32_t motor_mask;
    float rpm[32];         // RPM of all motors
    uint8_t rcin_chan_count;
    float  rcin[12];         // RC input 0..1
    double range;           // rangefinder value
    Vector3f_t bodyMagField;  // Truth XYZ magnetic field vector in body-frame. Includes motor interference. Units are milli-Gauss.
    Vector3f_t angAccel; // Angular acceleration in degrees/s/s about the XYZ body axes

    struct {
        // data from simulated laser scanner, if available
        struct vector3f_array points;
        struct float_array ranges;
    } scanner;

    float rangefinder_m[SITL_NUM_RANGEFINDERS];
    float airspeed_raw_pressure[AIRSPEED_MAX_SENSORS];

    struct {
        float speed;
        float direction;
    } wind_vane_apparent;

    bool is_lock_step_scheduled;

    // earthframe wind, from backends that know it
    Vector3f_t wind_ef;
};

struct BaroParm {
    Param_float noise;  // in metres
    Param_float drift;  // in metres per second
    Param_float glitch; // glitch in meters
    Param_int8  freeze; // freeze baro to last recorded altitude
    Param_int8  disable; // disable simulated barometers
    Param_int16 delay;  // barometer data delay in ms

    // wind coefficients
    Param_float wcof_xp;
    Param_float wcof_xn;
    Param_float wcof_yp;
    Param_float wcof_yn;
};

struct AirspeedParm {
    Param_float noise;  // pressure noise
    Param_float fail;   // airspeed value in m/s to fail to
    Param_float fail_pressure; // pitot tube failure pressure in Pa
    Param_float fail_pitot_pressure; // pitot tube failure pressure in Pa
    Param_float offset; // airspeed sensor offset in m/s
    Param_float ratio; // airspeed ratios
    Param_int8  signflip;
};

// EFI type
enum EFIType {
    EFI_TYPE_NONE = 0,
    EFI_TYPE_MS = 1,
};

// wind control
enum WindType {
    WIND_TYPE_SQRT = 0,
    WIND_TYPE_NO_LIMIT = 1,
    WIND_TYPE_COEF = 2,
};

typedef struct SIM* sim_t;
typedef const struct SIM* sim_const_t;
struct SIM {
    struct sitl_fdm state;

    // throttle when motors are active
    float throttle;

    // height above ground
    float height_agl;

    // Board Orientation (and inverse)
    matrix3f_t ahrs_rotation;
    matrix3f_t ahrs_rotation_inv;

    Param_float mag_noise;   // in mag units (earth field is 818)
    Param_Vector3f mag_mot;  // in mag units per amp
    Param_Vector3f mag_ofs[HAL_COMPASS_MAX_SENSORS];  // in mag units
    Param_Vector3f mag_diag[HAL_COMPASS_MAX_SENSORS];  // diagonal corrections
    Param_Vector3f mag_offdiag[HAL_COMPASS_MAX_SENSORS];  // off-diagonal corrections
    Param_int8 mag_orient[HAL_COMPASS_MAX_SENSORS];   // external compass orientation
    Param_int8 mag_fail[HAL_COMPASS_MAX_SENSORS];   // fail magnetometer, 1 for no data, 2 for freeze
    Param_float servo_speed; // servo speed in seconds

    Param_float sonar_glitch;// probability between 0-1 that any given sonar sample will read as max distance
    Param_float sonar_noise; // in metres
    Param_float sonar_scale; // meters per volt
    Param_int8 sonar_rot;  // from rotations enumeration

    Param_float drift_speed; // degrees/second/minute
    Param_float drift_time;  // period in minutes
    Param_float engine_mul;  // engine multiplier
    Param_int8  engine_fail; // engine servo to fail (0-7)

    Param_float gps_noise[2]; // amplitude of the gps altitude error
    Param_int16 gps_lock_time[2]; // delay in seconds before GPS gets lock
    Param_int16 gps_alt_offset[2]; // gps alt error
    Param_int8  gps_disable[2]; // disable simulated GPS
    Param_int16 gps_delay_ms[2];   // delay in milliseconds
    Param_int8  gps_type[2]; // see enum SITL::GPS::Type
    Param_float gps_byteloss[2];// byte loss as a percent
    Param_int8  gps_numsats[2]; // number of visible satellites
    Param_Vector3f gps_glitch[2];  // glitch offsets in lat, lon and altitude
    Param_int8  gps_hertz[2];   // GPS update rate in Hz
    Param_int8 gps_hdg_enabled[2]; // enable the output of a NMEA heading HDT sentence or UBLOX RELPOSNED
    Param_float gps_drift_alt[2]; // altitude drift error
    Param_Vector3f gps_pos_offset[2];  // XYZ position of the GPS antenna phase centre relative to the body frame origin (m)
    Param_float gps_accuracy[2];
    Param_Vector3f gps_vel_err[2]; // Velocity error offsets in NED (x = N, y = E, z = D)

    // initial offset on GPS lat/lon, used to shift origin
    Param_float gps_init_lat_ofs;
    Param_float gps_init_lon_ofs;
    Param_float gps_init_alt_ofs;

    Param_float batt_voltage; // battery voltage base
    Param_float batt_capacity_ah; // battery capacity in Ah
    Param_int8  rc_fail;     // fail RC input
    Param_int8  rc_chancount; // channel count
    Param_int8  float_exception; // enable floating point exception checks
    Param_int8  flow_enable; // enable simulated optflow
    Param_int16 flow_rate; // optflow data rate (Hz)
    Param_int8  flow_delay; // optflow data delay
    Param_int8  terrain_enable; // enable using terrain for height
    Param_int16 pin_mask; // for GPIO emulation
    Param_float speedup; // simulation speedup
    Param_int8  odom_enable; // enable visual odometry data
    Param_int8  telem_baudlimit_enable; // enable baudrate limiting on links
    Param_float flow_noise; // optical flow measurement noise (rad/sec)
    Param_int8  baro_count; // number of simulated baros to create
    Param_int8  imu_count; // number of simulated IMUs to create
    Param_int32 loop_delay; // extra delay to add to every loop
    Param_float mag_scaling[MAX_CONNECTED_MAGS]; // scaling factor
    Param_int32 mag_devid[MAX_CONNECTED_MAGS]; // Mag devid
    Param_float buoyancy; // submarine buoyancy in Newtons
    Param_int16 loop_rate_hz;
    Param_int32 on_hardware_output_enable_mask;  // mask of output channels passed through to actual hardware

#ifdef SFML_JOYSTICK
    Param_int8 sfml_joystick_id;
    Param_int8 sfml_joystick_axis[8];
#endif

    struct BaroParm baro[BARO_MAX_INSTANCES];

    struct AirspeedParm airspeed[AIRSPEED_MAX_SENSORS];

    Param_int8  efi_type;

    float wind_speed_active;
    float wind_direction_active;
    float wind_dir_z_active;

    Param_float wind_speed;
    Param_float wind_direction;
    Param_float wind_turbulance;
    Param_float wind_dir_z;
    Param_int8  wind_type; // enum WindLimitType
    Param_float wind_type_alt;
    Param_float wind_type_coef;

    Param_int16  mag_delay; // magnetometer data delay in ms

    // ADSB related run-time options
    Param_int16 adsb_plane_count;
    Param_float adsb_radius_m;
    Param_float adsb_altitude_m;
    Param_int8  adsb_tx;

    // Earth magnetic field anomaly
    Param_Vector3f mag_anomaly_ned; // NED anomaly vector at ground level (mGauss)
    Param_float mag_anomaly_hgt; // height above ground where anomally strength has decayed to 1/8 of the ground level value (m)

    // Body frame sensor position offsets
    Param_Vector3f imu_pos_offset;     // XYZ position of the IMU accelerometer relative to the body frame origin (m)
    Param_Vector3f rngfnd_pos_offset;  // XYZ position of the range finder zero range datum relative to the body frame origin (m)
    Param_Vector3f optflow_pos_offset; // XYZ position of the optical flow sensor focal point relative to the body frame origin (m)
    Param_Vector3f vicon_pos_offset;   // XYZ position of the vicon sensor relative to the body frame origin (m)

    // barometer temperature control
    Param_float temp_start;            // [deg C] Barometer start temperature
    Param_float temp_board_offset;     // [deg C] Barometer board temperature offset from atmospheric temperature
    Param_float temp_tconst;           // [deg C] Barometer warmup temperature time constant
    Param_float temp_baro_factor;

    Param_int8 thermal_scenario;

    // weight on wheels pin
    Param_int8 wow_pin;

    // vibration frequencies in Hz on each axis
    Param_Vector3f vibe_freq;

    // max frequency to use as baseline for adding motor noise for the gyros and accels
    Param_float vibe_motor;

    // amplitude scaling of motor noise relative to gyro/accel noise
    Param_float vibe_motor_scale;

    // what harmonics to generate
    Param_int16 vibe_motor_harmonics;

    // what servos are motors
    Param_int32 vibe_motor_mask;

    // minimum throttle for addition of ins noise
    Param_float ins_noise_throttle_min;

    struct {
        Param_float x;
        Param_float y;
        Param_float z;
        Param_int32 t;

        uint32_t start_ms;
    } shove;

    struct {
        Param_float x;
        Param_float y;
        Param_float z;
        Param_int32 t;

        uint32_t start_ms;
    } twist;

    Param_int8 gnd_behav;

    struct {
        Param_int8 enable;     // 0: disabled, 1: roll and pitch, 2: roll, pitch and heave
        Param_float length;    // m
        Param_float amp;       // m
        Param_float direction; // deg (direction wave is coming from)
        Param_float speed;     // m/s
    } wave;

    struct {
        Param_float direction; // deg (direction tide is coming from)
        Param_float speed;     // m/s
    } tide;

    // original simulated position
    struct {
        Param_float lat;
        Param_float lng;
        Param_float alt; // metres
        Param_float hdg; // 0 to 360
    } opos;

    Param_int8 _safety_switch_state;

    uint16_t irlock_port;

    time_t start_time_UTC;

    // simulated ship takeoffs
#if AP_SIM_SHIP_ENABLED
    struct sim_ship shipsim;
#endif

    // ESC telemetry
    Param_int8 esc_telem;

    // RPM when motors are armed
    Param_float esc_rpm_armed;

    struct {
        // LED state, for serial LED emulation
        struct {
            uint8_t rgb[3];
        } rgb[16][32];
        uint8_t num_leds[16];
        uint32_t send_counter;
    } led;

    Param_int8 led_layout;

    // vicon parameters
    Param_Vector3f vicon_glitch;   // glitch in meters in vicon's local NED frame
    Param_int8 vicon_fail;         // trigger vicon failure
    Param_int16 vicon_yaw;         // vicon local yaw in degrees
    Param_int16 vicon_yaw_error;   // vicon yaw error in degrees (added to reported yaw sent to vehicle)
    Param_int8 vicon_type_mask;    // vicon message type mask (bit0:vision position estimate, bit1:vision speed estimate, bit2:vicon position estimate)
    Param_Vector3f vicon_vel_glitch;   // velocity glitch in m/s in vicon's local frame

    // IMU control parameters
    Param_float gyro_noise[INS_MAX_INSTANCES];  // in degrees/second
    Param_Vector3f gyro_scale[INS_MAX_INSTANCES];  // percentage
    Param_float accel_noise[INS_MAX_INSTANCES]; // in m/s/s
    Param_Vector3f accel_bias[INS_MAX_INSTANCES]; // in m/s/s
    Param_Vector3f accel_scale[INS_MAX_INSTANCES]; // in m/s/s
    Param_Vector3f accel_trim;
    Param_float accel_fail[INS_MAX_INSTANCES];  // accelerometer failure value
    // gyro and accel fail masks
    Param_int8 gyro_fail_mask;
    Param_int8 accel_fail_mask;

    // Sailboat sim only
    Param_int8 sail_type;

    // Master instance to use servos from with slave instances
    Param_int8 ride_along_master;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
sim_t sim_get_singleton();

float sim_measure_distance_at_angle_bf(const Location *location, float angle);

/* report SITL state via MAVLink SIMSTATE*/
void sim_simstate_send(sim_const_t sim, mavlink_channel_t chan);

/* report SITL state via MAVLink SIM_STATE */
void sim_sim_state_send(sim_const_t sim, mavlink_channel_t chan);

// convert a set of roll rates from earth frame to body frame
// output values are in radians/second
void sim_convert_body_frame(double rollDeg, double pitchDeg,
                              double rollRate, double pitchRate, double yawRate,
                              double *p, double *q, double *r);

// convert angular velocities from body frame to
// earth frame.
// all inputs and outputs are in radians/s
Vector3f_t sim_convert_earth_frame(const matrix3f_t *dcm, const Vector3f_t *gyro);

// get the rangefinder reading for the desired rotation, returns -1 for no data
float sim_get_rangefinder(sim_t sim, uint8_t instance);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



